% Christian Hubicki

classdef OptTorqueControllerClass < ControllerClass
    
    properties (Constant)
        
        controller_type = 'no integral term';
        
    end
    
    properties
        time_vector; %= linspace(0,0.4,10);
        torque_vector; %= 50*[0 0 0 0 0 1 1 1 1 1];
        td_time;
    end
    
    methods
        
        function obj = OptTorqueControllerClass
            
        end
        
        function u = flight(obj, t, x)
            u = [0; 0];
%             u = [0; -20.0*x(ForceControlModelClass.doli)];% + -40*(x(ForceControlModelClass.oli)-360)];
        end
        
        function u = stance(obj, t, x, dxtdyt)    
            Fs = ForceControlModelClass.kl * ( ForceControlModelClass.RZ(x) - ForceControlModelClass.R(x) );
            
            u = [   0; 
                    Fs * ForceControlModelClass.rl / ForceControlModelClass.G + interp1(obj.time_vector, obj.torque_vector, t - obj.td_time, 'spline')];
            
%                 u = [   0; 
%                     interp1(obj.time_vector, obj.torque_vector, t - obj.td_time)];
                
                if(isnan(u(2)))
                    u(2) = 0;
                end
                
%                 u = [   0; 
%                         interp1(obj.time_vector, obj.torque_vector, t - obj.td_time)];
                
            
        end
        
        function lo( obj, t, x )
            
        end
        
        function td( obj, t, x )
            obj.td_time = t;
        end
        
    end
    
end
